//
// Created by jimmyteng on 19-4-10.
//
#include <visual_compass/visual_compass.h>
#include <gtest/gtest.h>

TEST(DomeTest, Demo) {
  cv::Mat K1(3, 3, CV_8U);
  auto image = cv::imread("../resource/testdata/1.png");
  EXPECT_EQ(image.empty(), false);
  float r[9]{-8.08247458e-03, 9.99967098e-01, 6.55866519e-04,
             -9.99333203e-01, -8.05399381e-03, -3.56122330e-02,
             -3.56057808e-02, -9.43264225e-04, 9.99365449e-01
  };

  float camera_matrix[9] = {2.79109619e+02, 0., 3.16118042e+02,
                            0., 2.79484833e+02, 2.37545059e+02,
                            0., 0., 1.};
  float distortion_coefficients[4] = {-7.02879429e-02, 2.03178778e-01, -2.42307484e-01,
                                      1.02940999e-01};


  cv::Mat K(3, 3, CV_32F, camera_matrix);
  cv::Mat DistCoef(1, 4, CV_32F, distortion_coefficients);
  cv::Mat R(3, 3, CV_32F, r);
  EXPECT_EQ(VisualCompass::GetInstance()->Initialize(K, DistCoef, R), NO_ERROR);
  double res, score;
  EXPECT_EQ(VisualCompass::GetInstance()->Detect(image, res, score), NO_ERROR);
  EXPECT_FLOAT_EQ(res, 3);
}